# Copyright 2016 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from std_msgs.msg import Float64MultiArray
import sensor_msgs.msg
import std_msgs.msg
import threading
import queue
import time
import numpy as np
import xarm

original_range = [850, 525]
target_range = [0, 750] #-135 degrees to +135 degrees

class xArmPub(Node):
    def __init__(self):
        super().__init__('xarm_publisher')
        self.publisher_ = self.create_publisher(sensor_msgs.msg.JointState, '/joint_states', 10)
        timer_period = 0.75  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0
        self.sync_data=6*[500]
        self.prev_sync_data = 6*[500]
        self.start_time = time.time()
        self.robot = self.connect_to_robot()

    def timer_callback(self):
        msg = sensor_msgs.msg.JointState()
        
        msg.header.stamp = self.get_clock().now().to_msg()
        t0 = time.time()
        for i in range(0,6):
            self.sync_data[i] = float(self.robot.getPosition(i+1))
        #get the serial input from the master
        self.prev_sync_data = self.sync_data
        msg.position = self.sync_data
        msg.position.append(t0)
        self.publisher_.publish(msg)

    def connect_to_robot(self):
        try:
            arm = xarm.Controller("USB")
            return arm
        except:
            print("Problem connecting to robot. Check connections")
            return None


        


def main(args=None):
    rclpy.init(args=args)

    xarm_pub = xArmPub()

    rclpy.spin(xarm_pub)
    xarm_pub.destroy_node()
    rclpy.shutdown()
    

if __name__ == '__main__':
    main()
